/*********************************************************************
* Software License Agreement (BSD License)
* 
*  Copyright (c) 2010, Jack Pien
*  All rights reserved.
* 
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
* 
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
* 
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#include "daido_face_locate.hpp"

#include <ros/ros.h>
#include <ros/package.h>
#include <image_transport/image_transport.h>
#include <std_msgs/UInt64.h>
#include <opencv/cvwimage.h>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <cv_bridge/CvBridge.h>
#include <sensor_msgs/Image.h>
#include <geometry_msgs/Point.h>

#include <boost/thread.hpp>
#include <boost/format.hpp>
#include <boost/shared_ptr.hpp>

using namespace __DaidoNs;

int
FaceLocate::Start( int argc, char** argv )
{
    ros::init(argc, argv, "daido_face_locate");
    ros::NodeHandle nh("~");
    if (nh.resolveName("image") == "/daido_face_locate/image") {
        ROS_WARN("image has not been remapped! Typical command-line usage:\n"
                 "\t$ ./daido_face_locate /daido_face_locate/image:=<image topic>");
    }

    image_transport::ImageTransport it(nh);
    image_transport::Subscriber sub;
  
    ros::Rate loop_rate(60);

    // Set up publisher
    img_pub_ = it.advertise( "/daido_face_locate/processed_image", 10 );
    face_location_pub_ = 
        nh.advertise<geometry_msgs::Point>( "/daido_face_locate/face_position", 
                                            5 );

    // Set up subscriber
    std::string topic = nh.resolveName("image");
    std::string transport = "raw";
    sub = it.subscribe(topic, 1, &FaceLocate::ImageCB, this, transport);

    // Initialize classifier
    std::string cascadefn = ros::package::getPath("daido_face_locate");
    cascadefn.append("/bin/haarcascade_frontalface_default.xml");
    cascade_ = (CvHaarClassifierCascade*)cvLoad( cascadefn.c_str() );

    while( nh.ok() ) {
        ros::spinOnce();
        loop_rate.sleep();
    }

    return 0;
}

// Image message callback
void
FaceLocate::ImageCB( const sensor_msgs::ImageConstPtr& msg )
{
    CvPoint2D32f obj32f;

    // Initialize return value
    obj32f.x = obj32f.y = -1.0f;

    if( img_bridge_.fromImage(*msg, "bgr8") && cascade_ != NULL ) {
        IplImage* tmp_img = img_bridge_.toIpl();
        CvSeq* obj;
        int i;
        float scale = 1.0f;
        CvRect largestObj;

        // Initialize the largest obj
        largestObj.x = largestObj.y = 0;
        largestObj.width = largestObj.height = 0;

        // Detect faces

        obj = cvHaarDetectObjects( tmp_img, cascade_, storage_, 
                                   1.2, 2, CV_HAAR_DO_CANNY_PRUNING );

        // Find largest face
        for( i = 0; i < obj->total; i++ ) {
            // extract the rectangles only 
            CvRect objrect = *(CvRect*)cvGetSeqElem( obj, i );

            // Larger than previous?
            if( (objrect.width*objrect.height*scale*scale) >
                largestObj.width * largestObj.height ) {
                largestObj.x = objrect.x * scale;
                largestObj.y = objrect.y * scale;
                largestObj.width = objrect.width * scale;
                largestObj.height = objrect.height * scale;
            }

        }

        // If we found a face, return the center of the largest
        if( obj->total > 0 ) {
            cvRectangle( tmp_img, 
                         cvPoint( largestObj.x, largestObj.y ),
                         cvPoint( (largestObj.x + largestObj.width),
                                  (largestObj.y + largestObj.height) ),
                         CV_RGB(255,0,0), 3 );

            obj32f.x = (float)largestObj.x + ((float)largestObj.width * 0.5f);
            obj32f.y = (float)largestObj.y + ((float)largestObj.height * 0.5f);
            //fprintf( stderr, "%f, %f\n", obj32f.x, obj32f.y );
            obj32f.x /= (float)tmp_img->width;
            obj32f.y /= (float)tmp_img->height;
            //fprintf( stderr, "%f, %f\n", obj32f.x, obj32f.y );
        } 

        sensor_msgs::ImagePtr msg = img_bridge_.cvToImgMsg( tmp_img, "bgr8" );
        img_pub_.publish( msg );
    } 

    // Publish the location of the face if found
    geometry_msgs::Point face_loc;
    face_loc.x = obj32f.x;
    face_loc.y = obj32f.y;
    face_loc.z = 0.0;
    face_location_pub_.publish( face_loc );
}

int main( int argc, char** argv )
{
    FaceLocate fl;
    int err = fl.Start( argc, argv );
    return err;
}
